#include "kalman.h"

void Kalman_Init(KalmanFilter *kf, double Q, double R)
{
    kf->X = 0.0f;
    kf->P = 1.0f;
    kf->Q = Q;
    kf->R = R;
    kf->K = 0.0f;
}

double Kalman_Update(KalmanFilter *kf, double measurement)
{
    // 预测误差协方差
    kf->P += kf->Q;

    // 计算卡尔曼增益
    kf->K = kf->P / (kf->P + kf->R);

    // 更新估计值
    kf->X += kf->K * (measurement - kf->X);

    // 更新误差协方差
    kf->P *= (1 - kf->K);

    return kf->X;
}
